#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     MUX,            sensorI2CCustomFastSkipStates)
#pragma config(Sensor, S4,     ProtoBoard,     sensorI2CCustomFastSkipStates9V)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop, reversed, encoder)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop, reversed, encoder)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     RightBack,     tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     LeftBack,      tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     RightFront,    tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     LeftFront,     tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     Lift1,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     Lift2,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C3_1,     Lift3,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C3_2,     Lift4,         tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    LeftGripper1,         tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    RightGripper1,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    LeftGripper2,         tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    RightGripper2,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    Forward,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_6,    Side,                 tServoContinuousRotation)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

#include "SmuxConfig.h"
#include "drivers/hitechnic-eopd.h"
#include "LiftManager.h"						//Automatic lift positioner
#include "3785_motion_functions.h"	//Motion fuctions such as forwards and backwards
#include "IRController.h"						//Include more precise autonomous IR reading function

void frontGripperAdjust(int position)
{
	servo[LeftGripper1]=255-position;
	servo[RightGripper1]=position;
}
void backGripperAdjust(int position)
{
	servo[LeftGripper2]=255-position;
	servo[RightGripper2]=position;
}

void initializeRobot()
{
	servoChangeRate[LeftGripper1]=5;
	servoChangeRate[LeftGripper2]=5;
	servoChangeRate[RightGripper1]=5;
	servoChangeRate[RightGripper2]=5;
	// Place code here to initialize servos to starting positions.
	// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
	frontGripperAdjust(128);
	backGripperAdjust(128);
	servo[Forward]=128;
	servo[Side]=128;
	HTEOPDsetShortRange(EOPD);
	return;
}
float IRLeftValue;
float IRRightValue;
int eopdval;

task sensors()
{
	while(true)
	{
		nxtDisplayTextLine(0,"%f",IRLeftValue);
		nxtDisplayTextLine(1,"%f",IRRightValue);
		hogCPU();
		IRLeftValue=IRGetACDir(IRLEFT);
		IRRightValue=IRGetACDir(IRRIGHT);
		eopdval = HTEOPDreadRaw(EOPD);
		releaseCPU();
		wait10Msec(2);
	}
}

int num=0;
bool continuing;
float dir=0;

task center()
{
	dir=IRLeftValue-(10-IRRightValue);
	while(true)
	{
		dir=IRLeftValue-(10-IRRightValue);
		if (dir>0)
		{
			servo[Side]=0;
			wait10Msec(2);
			servo[Side]=128;
		}
		if (dir<0)
		{
			servo[Side]=255;
			wait10Msec(2);
			servo[Side]=128;
		}
	}
	servo[Side]=128;
}

task main()
{
	initializeRobot();
	StartTask(sensors);
	waitForStart();   // wait for start of tele-op phase
	straight(50);
	continuing = true;
	clearDebugStream();
	while(continuing)
	{
		if(IRRightValue>=7)
		{
			num++;
		}
		else
		{
			num=0;
		}
		if(num>=2)
		{
			continuing=false;
		}
	}
	stopRobot();
	pause();
	initalizeEncoders();
	pause();
	straight(50,6);
	pause();
	turnRight(50,1950,true);
	stopRobot();
	StartTask(center);
	wait10Msec(200);
	//servo[Forward]=255;
	straight(30);
	continuing=true;
	clearDebugStream();
	while(continuing)
	{
		writeDebugStreamLine("%i",eopdval);
		if(eopdval>=15&&eopdval!=1023)
		{
			num++;
		}
		else
		{
			num=0;
		}
		if(num>=2)
		{
			continuing=false;
		}
	}
	//servo[Forward]=128;
	stopRobot();
	frontGripperAdjust(30);
	backGripperAdjust(30);
	pause();
	straight(-50,20);
}
